from stereo_processor_mutilprocess import ParallelStereoProcessor, time
# from stereo_processor import ParallelStereoProcessor, time
import time
import cv2
from stereo.utils.stereo_camera_threaded import StereoCameraCapture_threaded_ins
import sys
import numpy as np
import requests,json
cv2.setNumThreads(4)
cv2.setUseOptimized(True)
sender_flag = False
import os
# 获取当前进程或线程亲和CPU核
cpu_affinity = os.sched_getaffinity(0)
print(f"当前线程亲和CPU核: {sorted(cpu_affinity)}")
def send_data(kps_dict_2d, kps_2d_list, pts_3d_dict, pts_3d_list, left_image, right_image):
    kps_dict_2d = {k: v.tolist() if isinstance(v, np.ndarray) else v for k, v in kps_dict_2d.items()}
    pts_3d_dict = {k: v.tolist() if isinstance(v, np.ndarray) else v for k, v in pts_3d_dict.items()}

    result = {
        'data_2d':kps_dict_2d,
        'data_3d':pts_3d_dict
    }

    response = requests.post(
        "http://192.168.1.164:8000/get_data",
        headers={"Content-Type": "application/json"},
        data=json.dumps(result)
    )
def main():
    processor = ParallelStereoProcessor()
    if sender_flag:
        from stereo.utils.stereo_tcp_server import StereoDataSender
        from stereo.utils.stereo_tcp_server import StereoImagePublisher_ins
        sender = StereoDataSender()
    try:
        target_fps = 30
        frame_duration = 1 / target_fps
        while True:
            start_time = time.time()
            # 获取左右图像
            if sender_flag:
                if StereoImagePublisher_ins.client_socket is None:
                    time.sleep(0.1)
                    print("Waiting for client connection...")
                    continue
                else:
                    StereoCameraCapture_threaded_ins.clear_queue()
            else:
                StereoCameraCapture_threaded_ins.clear_queue()
            right_image, left_image, frame_time = StereoCameraCapture_threaded_ins.get_stereo_frames()
            # left_image, right_image, frame_time  = StereoCameraCapture_threaded_ins.get_stereo_frames()
            if right_image is None or left_image is None:
                time.sleep(0.1)
                raise KeyboardInterrupt

            t1 = time.time()
            for i in range(1):
                results = processor.process_frame(left_image, right_image)
            # print(f"Processing time: {time.time() - t1:.4f} seconds", round((time.time() - frame_time)*1000,2), results==None, StereoCameraCapture_threaded_ins.get_queue_size(), processor.depth_task_queue.qsize())
            send_end = time.time()
            total_time = send_end - t1  # t1为处理前，send_end为发送后
            # print(f"单帧从处理到socket发送完毕耗时: {total_time:.4f} 秒")
            # 可累计统计平均值
            if not hasattr(main, "costs"):
                main.costs = []
            main.costs.append(total_time)
            if len(main.costs) % 30 == 0:
                print(f"最近30帧平均耗时: {sum(main.costs[-30:])/30:.4f} 秒", StereoCameraCapture_threaded_ins.get_queue_size(), processor.depth_task_queue.qsize())
            if results is not None:
                kps_dict_2d, kps_2d_list, pts_3d_dict, pts_3d_list, left_image, right_image = results
                send_data(kps_dict_2d, kps_2d_list, pts_3d_dict, pts_3d_list, left_image, right_image)

            # if results is not None:
            #     kps_dict_2d, kps_2d_list, pts_3d_dict, pts_3d_list, left_image, right_image = results
            #     if sender_flag:
            #         sender.send_data(kps_dict_2d, kps_2d_list, pts_3d_dict, pts_3d_list, left_image, right_image)
                    
            elapsed = time.time() - start_time
            sleep_time = max(0, frame_duration - elapsed)
            # time.sleep(sleep_time)
    except KeyboardInterrupt:
        print("Stopping processor...")
        processor.stop()
        if sender_flag:
            while not sender.data_queue.empty():
                time.sleep(0.1)
            sender.stop()
        sys.exit(0)

if __name__ == "__main__":
    main() 